#ifndef TRACK_OBJECT_TRACK_H_
#define TRACK_OBJECT_TRACK_H_

#include "track/settings.h"
#include "track/id_generator.h"
#include "track/firstorder_lowpass_filter.h"
#include <deque>
#include <memory>
#include <vector>

namespace autodrive {
namespace perception {
class ObjectTrack {
 public:
  //   ObjectTrack(/* args */);
  ObjectTrack(TrackedObjectPtr obj);
  ObjectTrack(ObjectTrack const&) = delete;
  ObjectTrack& operator=(ObjectTrack const&) = delete;
  ObjectTrack(ObjectTrack&&) = delete;
  ObjectTrack& operator=(ObjectTrack&&) = delete;
  ~ObjectTrack();

  /// @brief current tracked object
  TrackedObjectPtr current_object_;

  /// @brief basic info
  uint8_t idx_;
  std::int32_t age_;
  std::int32_t total_visible_count_;
  std::int32_t consecutive_invisible_count_;

  /// @brief update track with object
  /// @param[IN] new_object: recent detected object for current updating
  /// @param[IN] time_diff: time interval from last updating
  /// @param[IN] is_utm_flag: flag of using utm or not
  /// @return nothing
  void UpdateWithObject(TrackedObjectPtr* new_object, double time_diff,
                        const MapTrackParameter& setting);

  /// @brief update track without object with given predicted state
  /// @param[IN] predict_state: given predicted state of track
  /// @param[IN] time_diff: time interval form last updating
  /// @return nothing
  void UpdateWithoutObject(Eigen::VectorXf const& predict_state,
                           double time_diff);

  /// @brief init id generator
  static void InitIdGenerator() noexcept { id_generator_.Init(); };

 private:
  /// @brief id generator
  static IdGenerator id_generator_;

  /// @brief history tracked object
  std::deque<TrackedObjectPtr> history_objects_;

  /// @brief 1st low pass filter
  FirstOrderLowPassFilter<Eigen::Vector3f> position_filter_;
  FirstOrderLowPassFilter<float> length_filter_;
  FirstOrderLowPassFilter<float> width_filter_;
  FirstOrderLowPassFilter<float> yaw_filter_;
};

using ObjectTrackPtr = ObjectTrack*;

class ObjectTrackSet {
 public:
  ObjectTrackSet();
  ~ObjectTrackSet();
  ObjectTrackSet(ObjectTrackSet const&) = delete;
  ObjectTrackSet& operator=(ObjectTrackSet const&) = delete;
  ObjectTrackSet(ObjectTrackSet&&) = delete;
  ObjectTrackSet& operator=(ObjectTrackSet&&) = delete;

  /// @brief set member by index
  /// @param index  data index
  inline std::int32_t Size() const { return tracks_.size(); }

  /// @brief set member by index
  /// @param index  data index
  inline void AddTrack(ObjectTrackPtr const& track) {
    tracks_.push_back(track);
  }

  /// @brief get maintained tracks
  /// @return maintained tracks
  inline std::vector<ObjectTrackPtr>& GetTracks() { return tracks_; }

  /// @brief get maintained tracks
  /// @return maintained tracks
  inline const std::vector<ObjectTrackPtr>& GetTracks() const {
    return tracks_;
  }

  /// @brief clear maintained tracks
  /// @return nothing
  inline void Clear() { tracks_.clear(); }

  /// @brief remove lost tracks
  /// @return number of removed tracks
  std::int32_t RemoveLostTracks();

 private:
  /// @brief tracks
  std::vector<ObjectTrackPtr> tracks_;
};

}  // namespace perception
}  // namespace autodrive

#endif